#!/usr/bin/env python3
# coding:utf-8

import asyncio
import struct
import time
from loguru import logger
from AGV_F50A_protocol import *
from common import show_hex


class AGV_F50A:
    """
    牧星F50A 潜伏车
    """

    def __init__(self, agvServer):
        self.agvServer = agvServer
        self.init_data()
        self.init_timer_task()

    def init_data(self):
        self.Client_Set = {}
        self.recv_callback_map = {
            E_DeviceCmd.Login: self.handler_recv_login,
            E_DeviceCmd.HeartBeat: self.handler_recv_HeartBeat,
            E_DeviceCmd.StateCompleteResp: self.handler_recv_StateCompleteResp,
            E_DeviceCmd.SignalCmd_Resp: self.handler_recv_SignalCmd_Resp,
            E_DeviceCmd.PeriodicReport: self.handler_recv_PeriodicReport,
            E_DeviceCmd.FaultPackage: self.handler_recv_FaultPackage,
            E_DeviceCmd.RealtimePackage: self.handler_recv_RealtimePackage,
            E_DeviceCmd.QRCodePath_Act_Resp_OK: self.handler_QRCodePath_Act_OK,
            E_DeviceCmd.QRCodePath_Act_Resp_NG: self.handler_QRCodePath_Act_NG,
        }

    def init_timer_task(self):
        """
        协程/异步定时任务
        """
        # 1. 已连接的客户端每个2s发送心跳包
        self.agvServer.async_scheduler.add_job(
            self.broadcast_client_heartbeat,
            "interval",
            seconds=2,
            id="broadcast_heartbeat",
        )

    def del_client(self, socket):
        """
        删除客户端, 通过socket判断
        """
        for key in self.Client_Set:
            if self.Client_Set[key].socket == socket:
                self.Client_Set.pop(key)

    async def handler_recv_msg(self, recv, socket):
        """
        处理: 收到的数据
        """
        frame = Frame().from_bytes(recv)
        if frame == None:
            logger.debug(f"报文解析失败")
            # show_hex(recv)
            return

        cmd_code = frame.cmd_code
        if (cmd_code != E_DeviceCmd.Login) and (frame.client_id not in self.Client_Set):
            logger.info(f"未登录的客户端: {frame.client_id:02X}, cmd:{cmd_code:02X}")
            await self.hal_send_activate(frame, socket)
            return

        # 根据 命令码调用回调函数
        if cmd_code in self.recv_callback_map:
            # logger.debug(f"收到报文cmd_code: {frame.cmd_code:02X}")
            await self.recv_callback_map[cmd_code](frame, socket)
        else:
            logger.info(f"未知命令字cmd_code: {cmd_code:02X}")

    async def handler_recv_login(self, frame, socket):
        """
        agv登录请求
        """
        # 1.解析登录信息
        login_frame = LoinFrame().unpack(frame.data)
        client = AgvClient(
            client_id=frame.client_id,
            socket=socket,
            login_frame=login_frame,
        )
        # 2.保存客户端
        self.Client_Set[frame.client_id] = client

        # 3.回复
        host_id = E_FrameHead.Host_Id
        resp = struct.pack("<HBBBBB", host_id, 0, 0, 0, 0, 0)
        reply_frame = Frame(
            client_id=frame.client_id,
            cmd_code=E_FrameCmd.RespLogin,
            data=resp,
        )
        logger.info(f"recv [id: {frame.client_id:02X}] {str(login_frame)}")
        await self.agvServer.send_data(socket, reply_frame.to_bytes())
        await asyncio.sleep(0.5)
        await self.hal_send_HeartBeat(frame, socket)

    async def handler_recv_HeartBeat(self, frame, socket):
        """
        接收: 心跳包
        """
        heartBeat_frame = HeartBeatFrame().unpack(frame.data)
        logger.debug(f"recv [id: {frame.client_id:02X}] {str(heartBeat_frame)}")
        await self.hal_send_HeartBeat(frame, socket)

    async def hal_send_HeartBeat(self, frame, socket):
        """
        发送: 心跳包响应
        """
        resp = b""
        reply_frame = Frame(
            client_id=frame.client_id,
            cmd_code=E_FrameCmd.HeartBeat,
            data=resp,
        )
        await self.agvServer.send_data(socket, reply_frame.to_bytes())

    async def handler_recv_StateCompleteResp(self, frame, socket):
        """
        状态完成数据包：（顶升完成、下降完成、转弯完成、旋转货架完成、扫描货架码完成、路径结束、充电对接完成、充满电
        """
        stateComplete_frame = StateCompleteFrame().unpack(frame.data)
        logger.info(f"recv [id: {frame.client_id:02X}] {str(stateComplete_frame)}")
        await self.send_reply_StateComple(frame, socket)

    async def handler_recv_SignalCmd_Resp(self, frame, socket):
        """
        命令码 回复包
        """
        signalCmd, args = struct.unpack_from("<Bh", frame.data)
        logger.debug(
            f"recv [id: {frame.client_id:02X}] signalCmd_Resp| cmd:{signalCmd:02X} args:{args:04X}"
        )

    async def send_reply_StateComple(self, frame, socket):
        """
        状态完成数据包 响应
        """
        resp = b""
        reply_frame = Frame(
            client_id=frame.client_id,
            cmd_code=E_DeviceCmd.QRCodePath_Act_Resp_OK,
            data=resp,
        )
        await self.agvServer.send_data(socket, reply_frame.to_bytes())

    async def handler_recv_PeriodicReport(self, frame, socket):
        """
        周期性状态数据包
        """
        client = self.Client_Set[frame.client_id]
        client.periodic_state.unpack(frame.data)
        logger.debug(f"recv [id: {frame.client_id:02X}] {str(client.periodic_state)}")

    async def handler_recv_RealtimePackage(self, frame, socket):
        """
        实时数据包:
            地址码id,x,y,z偏移
            货架码id + 当前状态位
        """
        client = self.Client_Set[frame.client_id]
        client.realtime_state.unpack(frame.data)
        logger.debug(f"recv [id: {frame.client_id:02X}] {str(client.realtime_state)}")

    async def handler_recv_FaultPackage(self, frame, socket):
        client = self.Client_Set[frame.client_id]
        client.fault_frame.unpack(frame.data)
        logger.error(f"recv [id: {frame.client_id:02X}] {str(client.fault_frame)}")

    async def handler_QRCodePath_Act_OK(self, frame, socket):
        last_cmd_code = frame.data[0]
        logger.warning(
            f"recv [id: {frame.client_id:02X}] last path cmd: {last_cmd_code:02X} ok"
        )

    async def handler_QRCodePath_Act_NG(self, frame, socket):
        last_cmd_code = frame.data[0]
        logger.error(
            f"recv [id {frame.client_id:02X}] last path cmd: {last_cmd_code:02X} ng"
        )

    async def hal_send_activate(self, frame, socket=None):
        """
        发送: 重新登录-激活报文
        """
        send_frame = Frame(
            client_id=frame.client_id,
            cmd_code=E_FrameCmd.Activate,
            data=b"",
        )
        if socket == None:
            client = self.Client_Set[frame.client_id]
            socket = client.socket

        await self.agvServer.send_data(socket, send_frame.to_bytes())

    async def handler_device_QRCodePath_Act_Resp(self, recv, client_id, client_socket):
        result = recv[11]
        act_id = recv[12]
        if result == E_DeviceCmd.QRCodePath_Act_Resp_NG:
            print(f"执行命令失败: act_id:{act_id:02x}")
            return

        data = struct.pack("<BI", act_id, 0x0)
        frame = Frame(
            client_id=client_id,
            cmd_code=E_DeviceCmd.QRCodePath_Act_Resp_OK,
            data=data,
        )
        send_data = frame.to_bytes()
        client_socket.write(send_data)
        await client_socket.drain()

    def hal_send_SignalCmd(self, client_id, signal_cmd, value):
        if client_id == 0:
            print("client_id 不能为0")
            return None

        data = struct.pack("<BH", signal_cmd, value)
        frame = Frame(
            header=E_FrameHead.TcpServer_Id,
            client_id=client_id,
            cmd_code=E_FrameCmd.SignalCmd,
            data=data,
        )
        send_data = frame.to_bytes()

        client = self.Client_Set[client_id]
        self.agvServer.async_task_send_data(client.socket, send_data)

    def hal_get_agv_batteryInfo(self, client_id):
        client = self.Client_Set[client_id]
        battery_info = {
            "agv_id": client_id,
            "voltage": int(client.periodic_state.battery_voltage / 10),
            "current": client.login_frame.max_charge_current,
            "battery_type": client.login_frame.battery_type,
        }
        return battery_info

    def hal_charge_cmd(self, client_id, enable: bool):
        if client_id == 0:
            print("client_id 不能为0")
            return None

        code = 0
        if enable:
            code = E_PathAction.Charger
            logger.warning(f"start充电: {client_id:02X}")
        else:
            logger.warning(f"stop充电: {client_id:02X}")
            code = E_PathAction.Discharger

        qrCodePath = QRCodePath(
            address=0x2,
            cmd=code,
            type=E_PathAction_DataType.ShortType,
            data=0x0,
        )
        data = struct.pack("<B", 1)
        data += struct.pack(
            "<IBBH",
            qrCodePath.address,
            qrCodePath.cmd,
            qrCodePath.type,
            qrCodePath.data,
        )

        frame = Frame(
            header=E_FrameHead.TcpServer_Id,
            client_id=client_id,
            cmd_code=E_FrameCmd.UpdatePath,
            data=data,
        )
        send_data = frame.to_bytes()
        logger.debug(f"充电 {client_id:02X}, ")
        show_hex(send_data, f"充电:{client_id:02X}")

        client = self.Client_Set[client_id]
        self.agvServer.async_task_send_data(client.socket, send_data)

    def hal_update_path(self, client_id, QRCodePath_List):
        if client_id == 0:
            print("client_id 不能为0")
            return None

        data = struct.pack("<B", len(QRCodePath_List))
        for QrCodePath in QRCodePath_List:
            data += struct.pack(
                "<IBBH",
                QrCodePath.address,
                QrCodePath.cmd,
                QrCodePath.type,
                QrCodePath.data,
            )

        frame = Frame(
            header=E_FrameHead.TcpServer_Id,
            client_id=client_id,
            cmd_code=E_FrameCmd.UpdatePath,
            data=data,
        )
        send_data = frame.to_bytes()

        client = self.Client_Set[client_id]
        self.agvServer.async_task_send_data(client.socket, send_data)

    def gen_QRCodePath_by_shortCmd(self, shortCmd):
        path_action = []
        curPath: int = 0
        idx: int = 0
        for cmd in shortCmd:
            if idx % 2 == 0:
                curPath = int(cmd)
            else:
                # 控制 + 移动
                if cmd == "-":
                    codePath = QRCodePath(
                        curPath,
                        E_PathAction.LineMove,
                        E_PathAction_DataType.IntType,
                        1150,
                    )
                elif cmd == "=":
                    codePath = QRCodePath(
                        curPath,
                        E_PathAction.LineMove,
                        E_PathAction_DataType.IntType,
                        1350,
                    )
                elif cmd == "#":
                    codePath = QRCodePath(
                        curPath, E_PathAction.Stop, E_PathAction_DataType.ShortType, 0
                    )
                elif cmd == "^":
                    codePath = QRCodePath(
                        curPath,
                        E_PathAction.Lifting_UpDown,
                        E_PathAction_DataType.ShortType,
                        200,
                    )
                elif cmd == "v":
                    codePath = QRCodePath(
                        curPath,
                        E_PathAction.Lifting_UpDown,
                        E_PathAction_DataType.ShortType,
                        0,
                    )
                # 转向
                elif cmd == "i":
                    codePath = QRCodePath(
                        curPath,
                        E_PathAction.WheelSteering,
                        E_PathAction_DataType.ShortType,
                        0,
                    )
                elif cmd == "k":
                    codePath = QRCodePath(
                        curPath,
                        E_PathAction.WheelSteering,
                        E_PathAction_DataType.ShortType,
                        180,
                    )
                elif cmd == "j":
                    codePath = QRCodePath(
                        curPath,
                        E_PathAction.WheelSteering,
                        E_PathAction_DataType.ShortType,
                        270,
                    )
                elif cmd == "l":
                    codePath = QRCodePath(
                        curPath,
                        E_PathAction.WheelSteering,
                        E_PathAction_DataType.ShortType,
                        90,
                    )
                else:
                    print(f"未知命令: {cmd}")
                    continue
                path_action.append(codePath)

            idx += 1
        return path_action

    async def broadcast_client_heartbeat(self):
        if len(self.Client_Set) == 0:
            return

        # 遍历副本: fix RuntimeError: dictionary changed size during iteration
        for client_id in list(self.Client_Set):
            if not client_id in self.Client_Set:
                continue
            client = self.Client_Set[client_id]
            frame = Frame(client_id=client_id)
            try:
                logger.debug(f"广播心跳 {client_id:02X}, ")
                await self.hal_send_HeartBeat(frame, client.socket)
            except Exception as e:
                logger.error(f"广播心跳失败: {e} -> {client_id}")
                self.Client_Set.pop(client_id)
                client.socket.close()
                await client.socket.wait_closed()
